#include "sensordatathread.h"
#include <QDateTime>
#include <QDebug>

SensorDataThread::SensorDataThread(QObject *parent)
    : QThread(parent)
    , m_running(true)
    , m_startTime(QDateTime::currentDateTime())
    , m_sensorConnector(nullptr)
{
    initSensorConnector();
}

SensorDataThread::~SensorDataThread()
{
    stop();
    cleanupSensorConnector();
}

void SensorDataThread::initSensorConnector()
{
    haptron::ConnectorConfig sc;
    sc.sensor_type = haptron::SensorType::Photon_FINGER;
    sc.com_pro = haptron::CommucationProtocol::Serial;
    sc.port = std::string("COM14");

    try {
        m_sensorConnector = new haptron::SensorConnector(sc);
        bool ret = m_sensorConnector->connect();

        if(!ret)
        {
            qDebug()<< "sensor connect fail";
            cleanupSensorConnector();
        }
    } catch (const std::exception& e) {
        qDebug() << "sensor init fail" << e.what();
        cleanupSensorConnector();
    }
}

void SensorDataThread::cleanupSensorConnector()
{
    if (m_sensorConnector) {
        m_sensorConnector->close();
        delete m_sensorConnector;
        m_sensorConnector = nullptr;
    }
}

void SensorDataThread::stop()
{
    m_running = false;
    wait(); // 等待线程完成当前工作
}

void SensorDataThread::run()
{
    if (!m_sensorConnector) {
        qDebug() << "传感器未初始化，无法运行数据采集";
        return;
    }
    //qDebug() << "yes";
    float fx = 0.0f, fy = 0.0f, fz = 0.0f;
    float mx = 0.0f, my = 0.0f, mz = 0.0f;

    while (m_running) {
        // 计算从开始到现在的时间
        //qDebug() << "yes";
        QDateTime currentTime = QDateTime::currentDateTime();
        double elapsedSeconds = m_startTime.msecsTo(currentTime) / 1000.0;

        try {
            // 从传感器获取数据
            if (m_sensorConnector) {
                m_sensorConnector->get_data(fx, fy, fz, mx, my, mz);

                // 力解耦计算
                haptron::photon_finger_decouple(0.0f, 0.0f, 0.005f, fz, mx, my, fx, fy);

                // 发送数据到主线程
                emit sensorDataUpdated(elapsedSeconds, fx, fy, fz);
            }
        } catch (const std::exception& e) {
            qDebug() << "传感器数据获取异常:" << e.what();
            msleep(100); // 异常情况下延长等待时间
            continue;
        }

        // 暂停10ms (100Hz 更新率)
        msleep(10);
    }
}
